"""
传感器管理器
统一管理所有传感器
"""

import asyncio
import json
import time
from typing import Dict, Any, Optional
from .ultrasonic_sensor import Measurement
from .camera_sensor import CameraSensor
from .imu_sensor import IMUSensor


class SensorManager:
    """传感器管理器类"""

    def __init__(self, config: Dict[str, Any]):
        """
        初始化传感器管理器

        Args:
            config: 配置字典，包含所有传感器参数
        """
        self.config = config

        # 初始化超声波传感器
        self.sonar = Measurement(
            config.get("sonar_trig", 12), config.get("sonar_echo", 16)
        )
        self.distance = 0.0
        self.distance_ok = False

        # 初始化摄像头传感器
        self.camera = CameraSensor(config)

        # 初始化IMU传感器
        self.imu = IMUSensor(0x79)
        self.imu.set_gyro_range(self.imu.GYRO_RANGE_250DEG)
        self.imu.set_accel_range(self.imu.ACCEL_RANGE_2G)

        # 启动传感器数据采集任务
        self._start_sensor_tasks()

    def _start_sensor_tasks(self) -> None:
        """启动所有传感器数据采集任务"""
        asyncio.create_task(self._update_distance())

    async def _update_distance(self) -> None:
        """更新超声波距离数据"""
        while True:
            try:
                if not self.distance_ok:
                    self.distance = self.sonar.distance_metric(
                        self.sonar.raw_distance(2, 0.08)
                    )
                    self.distance_ok = True
                await asyncio.sleep(0.1)
            except Exception as e:
                print(f"超声波数据更新失败: {e}")
                await asyncio.sleep(1)

    async def get_sensor_data(self, sensor_type: str) -> Dict[str, Any]:
        """
        获取指定传感器的数据

        Args:
            sensor_type: 传感器类型，可选值：'sonar', 'camera', 'imu'

        Returns:
            传感器数据字典
        """
        if sensor_type == "sonar":
            return {"distance": self.distance, "timestamp": time.time()}
        elif sensor_type == "camera":
            return await self.camera.get_sensor_data()
        elif sensor_type == "imu":
            return await self.get_imu_data()
        else:
            raise ValueError(f"未知的传感器类型: {sensor_type}")

    async def get_imu_data(self) -> Dict[str, Any]:
        """
        获取IMU数据

        Returns:
            包含加速度和角速度数据的字典
        """
        accel_data = self.imu.get_accel_data(g=True)
        gyro_data = self.imu.get_gyro_data()

        return {
            "acceleration": accel_data,
            "gyroscope": gyro_data,
            "timestamp": time.time(),
        }

    async def get_battery_status(self) -> Dict[str, Any]:
        """
        获取电池状态（待实现）

        Returns:
            包含电池状态信息的字典
        """
        # TODO: 实现电池状态获取
        return {
            "voltage": 12.0,
            "current": 0.5,
            "percentage": 100,
            "timestamp": time.time(),
        }

    async def close(self) -> None:
        """关闭所有传感器"""
        await self.camera.close()
        # 可以在这里添加其他传感器的关闭逻辑
